
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mp_autotune.h
  * @author     baiyang
  * @date       2023-6-29
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <pid/pid.h>
#include <pid/pid_info.h>

#include <logger/ap_log.h>
#include <parameter/param.h>
#include <vehicle/mb_fixed_wing.h>
#include <common/filter/lpfilter.h>
#include <common/filter/ModeFilter.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
struct ATGains {
    Param_float tau;
    Param_int16 rmax_pos;
    Param_int16 rmax_neg;
    float FF, P, I, D, IMAX;
    float flt_T, flt_E, flt_D;
};

enum ATType {
    MP_AUTOTUNE_ROLL  = 0,
    MP_AUTOTUNE_PITCH = 1,
    MP_AUTOTUNE_YAW = 2,
};

// the demanded/achieved state
enum ATState {
    MP_AT_IDLE,
    MP_AT_DEMAND_POS,
    MP_AT_DEMAND_NEG
};

// the demanded/achieved state
enum ATAction {
    MP_AT_NONE,
    MP_AT_LOW_RATE,
    MP_AT_SHORT,
    MP_AT_RAISE_PD,
    MP_AT_LOWER_PD,
    MP_AT_IDLE_LOWER_PD,
    MP_AT_RAISE_D,
    MP_AT_RAISE_P,
    MP_AT_LOWER_D,
    MP_AT_LOWER_P
};

#pragma pack(1)
struct log_ATRP {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t type;
    uint8_t state;
    float actuator;
    float P_slew;
    float D_slew;
    float FF_single;
    float FF;
    float P;
    float I;
    float D;
    uint8_t action;
    float rmax;
    float tau;
};
#pragma pack()

typedef struct mp_autotune *mp_autotune_t;

struct mp_autotune {
    // are we running?
    bool running;

    // the current gains
    struct ATGains *current;
    PID_ctrl *rpid;

    // what type of autotune is this
    enum ATType type;

    mb_fixed_wing_const_t aparm;

    // values to restore if we leave autotune mode
    struct ATGains restore;
    struct ATGains last_save;

    // last logging time
    uint32_t last_log_ms;

    enum ATState state;

    enum ATAction action;

    // when we entered the current state
    uint32_t state_enter_ms;

    // 5 point mode filter for FF estimate
    struct ModeFilterFloat_Size5 ff_filter;

    LowPassFiltFloat actuator_filter;
    LowPassFiltFloat rate_filter;
    LowPassFiltFloat target_filter;

    // separate slew limiters for P and D
    float slew_limit_max, slew_limit_tau;
    struct slew_limiter slew_limiter_P;
    struct slew_limiter slew_limiter_D;

    float max_actuator;
    float min_actuator;
    float max_rate;
    float min_rate;
    float max_target;
    float min_target;
    float max_P;
    float max_D;
    float min_Dmod;
    float max_Dmod;
    float max_SRate_P;
    float max_SRate_D;
    float FF_single;
    uint16_t ff_count;
    float dt;
    float D_limit;
    float P_limit;
    uint32_t D_set_ms;
    uint32_t P_set_ms;
    uint8_t done_count;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
mp_autotune_t mp_autotune_probe(struct ATGains *_gains, enum ATType type, mb_fixed_wing_const_t parms, PID_ctrl *rpid, float dt);

// constructor
void mp_autotune_ctor(mp_autotune_t autotune, struct ATGains *_gains, enum ATType type, mb_fixed_wing_const_t parms, PID_ctrl *rpid, float dt);

// called when autotune mode is entered
void mp_autotune_start(mp_autotune_t autotune);

// called to stop autotune and restore gains when user leaves
// autotune
void mp_autotune_stop(mp_autotune_t autotune);

// update called whenever autotune mode is active. This is
// called at the main loop rate
void mp_autotune_update(mp_autotune_t autotune, pid_info_t pid_info, float scaler, float angle_err_deg);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



